/*
 * Team D state control code.
 * @author Derek Leung
 */

#include "peripheral.c"
#include "drive_base.c"

/*TODO semaphores*/

const READY = 1;
const BUSY = 0;
int auto_state = READY;

bool blocking = false;

void auto_intake(int direction) {
  //kills intake if direction is 0
  intake_direction = direction;
  if(direction != 0) {
    StartTask(regulate_intake);
  } else {
    StopTask(regulate_intake);
  }
}

void auto_move(int inches) {
  auto_state = BUSY;
  drive_base_target_linear = inches;
  if(!drive_base_move) {
    drive_base_move = true;
    StartTask(regulate_drive_base);
  }
}

void auto_arm(double degrees) {
  auto_state = BUSY;
	arm_level_target = degrees;
	if(!arm_move) {
    arm_move = true;
    StartTask(regulate_arm);
  }
}

void auto_turn(double degrees) {} //TODO


void auto_score(int numbers, int level) {
  if(!arm_move) {
    StartTask(regulate_arm);
  }
  arm_move = true;
  arm_score = true;
  auto_state = BUSY;
}

void auto_hover() {
  if(!arm_move) {
    StartTask(regulate_arm);
  }
  arm_move = true;
  arm_hover = true;
  arm_score = true;
  auto_state = BUSY;
}

void auto_wait(int seconds)
{
  //wait1Msec(seconds*1000);
} //TODO TEST!!!!!

void auto_block() {
  blocking = true;
  auto_state = BUSY;
}

//placeholder loop functionality
void pl() {//TODO integrate into full loop
  int button; //TODO integrate with sensor registration
  while(true) {
    if(arm_target_reached && drive_base_target_reached) {
      auto_state = READY;
    }
    if(SensorValue(button)) {
      if(blocking) {
        auto_state = READY;
        blocking = false;
      }
    }
  }
}
